#include "core/include/types.h"
#include "core/include/aks_processor.h"
#include "core/include/process.h"
#include "core/include/aks_string.h"

#include "app/cv/include/aksImage.h"
#include "app/cv/include/aksDraw.h"

#include "include/tinySystem.h"

void tinySystem_tsk(void *param);
BOOL tinySystem_tsk_wakeup(void *param);

PROCESS tinySystem_process =
{
	tinySystem_tsk,
    NULL,
    tinySystem_tsk_wakeup,
    NULL,
    0
};

LIST_ITEM tinySystemItm
=
{
    NULL,
    &tinySystem_process
};


#define MAX_SYSTEMS 10

#define TINY_SYSTEM_NORMAL  0x11
#define TINY_SYSTEM_INVALID 0xFF

BYTE tinySystemTable[MAX_SYSTEMS];


BYTE cmdSeq[]=
{
	DRIVE_R | DRIVE_L | DRIVE_R_FWD | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_FWD | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_FWD | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_BCK | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_BCK | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_BCK | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_BCK | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_BCK | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_FWD | DRIVE_L_FWD,
	DRIVE_R | DRIVE_L | DRIVE_R_FWD | DRIVE_L_FWD
};


TINY_GPIO r_gpio0, r_gpio1, r_gpio2;
TINY_GPIO l_gpio0, l_gpio1, l_gpio2;

void setup_RightDriver(BYTE driveCmd)
{
    driveCmd = driveCmd & 0x0F;

    // drive
    if(driveCmd & DRIVE)
    {
    	r_gpio1.bit.DIR    = TINY_GPIO_OUT;
    	r_gpio1.bit.OUTPUT = TINY_GPIO_H;
    	// fwd
    	if(driveCmd & DRIVE_FWD)
    	{
        	r_gpio0.bit.DIR    = TINY_GPIO_OUT;
        	r_gpio0.bit.OUTPUT = TINY_GPIO_H;

        	r_gpio2.bit.DIR    = TINY_GPIO_OUT;
        	r_gpio2.bit.OUTPUT = TINY_GPIO_L;
    	}
    	// back
    	else
    	{
        	r_gpio0.bit.DIR    = TINY_GPIO_OUT;
        	r_gpio0.bit.OUTPUT = TINY_GPIO_L;

        	r_gpio2.bit.DIR    = TINY_GPIO_OUT;
        	r_gpio2.bit.OUTPUT = TINY_GPIO_H;

    	}
    }
    // stop
    else
    {
    	r_gpio0.bit.DIR    = TINY_GPIO_OUT;
    	r_gpio0.bit.OUTPUT = TINY_GPIO_L;

    	r_gpio1.bit.DIR    = TINY_GPIO_OUT;
    	r_gpio1.bit.OUTPUT = TINY_GPIO_L;

    	r_gpio2.bit.DIR    = TINY_GPIO_OUT;
    	r_gpio2.bit.OUTPUT = TINY_GPIO_L;
    }
}

void setup_LeftDriver(BYTE driveCmd)
{
    driveCmd = driveCmd & 0x0F;

    // drive
    if(driveCmd & DRIVE)
    {
    	l_gpio1.bit.DIR    = TINY_GPIO_OUT;
    	l_gpio1.bit.OUTPUT = TINY_GPIO_H;
    	// fwd
    	if(driveCmd & DRIVE_FWD)
    	{
        	l_gpio0.bit.DIR    = TINY_GPIO_OUT;
        	l_gpio0.bit.OUTPUT = TINY_GPIO_H;

        	l_gpio2.bit.DIR    = TINY_GPIO_OUT;
        	l_gpio2.bit.OUTPUT = TINY_GPIO_L;
    	}
    	// back
    	else
    	{
        	l_gpio0.bit.DIR    = TINY_GPIO_OUT;
        	l_gpio0.bit.OUTPUT = TINY_GPIO_L;

        	l_gpio2.bit.DIR    = TINY_GPIO_OUT;
        	l_gpio2.bit.OUTPUT = TINY_GPIO_H;

    	}
    }
    // stop
    else
    {
    	l_gpio0.bit.DIR    = TINY_GPIO_OUT;
    	l_gpio0.bit.OUTPUT = TINY_GPIO_L;

    	l_gpio1.bit.DIR    = TINY_GPIO_OUT;
    	l_gpio1.bit.OUTPUT = TINY_GPIO_L;

    	l_gpio2.bit.DIR    = TINY_GPIO_OUT;
    	l_gpio2.bit.OUTPUT = TINY_GPIO_L;
    }
}

BYTE currentMode;
void tinySystemDriver_SetCmd(BYTE cmd)
{
	currentMode = cmd;

	return;
}


#define TINY_RIGHT_DRIVER 0x01
#define TINY_LEFT_DRIVER  0x02

BYTE idx;
DWORD sleepTick;
void tinySystem_tsk(void *param)
{
	// test code
	BYTE cmd[3] = {0x00, 0x00, 0x00};

//	currentMode = cmdSeq[idx++];
//	idx = idx >= 10 ? 0 : idx;

	setup_RightDriver((currentMode>>0) & 0x0F);
	setup_LeftDriver ((currentMode>>4) & 0x0F);

	// setup cmd and send
	// Right
	cmd[1] = TINY_GPIO0_SETTING;
	cmd[2] = r_gpio0.byte;
	I2C_transmitBytes(TINY_RIGHT_DRIVER, cmd, 3);

	cmd[1] = TINY_GPIO1_SETTING;
	cmd[2] = r_gpio1.byte;
	I2C_transmitBytes(TINY_RIGHT_DRIVER, cmd, 3);

	cmd[1] = TINY_GPIO2_SETTING;
	cmd[2] = r_gpio2.byte;
	I2C_transmitBytes(TINY_RIGHT_DRIVER, cmd, 3);

	// Left
	cmd[1] = TINY_GPIO0_SETTING;
	cmd[2] = l_gpio0.byte;
	I2C_transmitBytes(TINY_LEFT_DRIVER, cmd, 3);

	cmd[1] = TINY_GPIO1_SETTING;
	cmd[2] = l_gpio1.byte;
	I2C_transmitBytes(TINY_LEFT_DRIVER, cmd, 3);

	cmd[1] = TINY_GPIO2_SETTING;
	cmd[2] = l_gpio2.byte;
	I2C_transmitBytes(TINY_LEFT_DRIVER, cmd, 3);




	BYTE dat[1];
	WORD ADC;

	cmd[1] = 0x1E;
	I2C_receiveBytes(0x03, cmd, 2, dat, 1);

	ADC = dat[0];

	cmd[1] = 0x1F;
	I2C_receiveBytes(0x03, cmd, 2, dat, 1);

	ADC = ADC << 8 | dat[0];

	BYTE txt[100];
	BYTE *next = txt;
	next = write_Str((const BYTE *)"ADC = ", next, 100);
	next = writeUHex(ADC, next, 100);
	next = write_Str((const BYTE *)"\r\n", next, 100);

	UART_transmitString(UART_DBG, txt);

	sleepTick = TIMER_systemTick();
	sleepCurrentProcess(tinySystem_tsk_wakeup, NULL);
}


BOOL tinySystem_tsk_wakeup(void *param)
{
	return ((TIMER_systemTick() - sleepTick) > 1000) ? TRUE : FALSE;
}


BOOL init_tinySystem_tsk(void)
{
	BYTE slvAdr = 0x01;

	BYTE cmd[3] = {0x00, 0x03, 0x00};
	BYTE dat[1];

	UART_transmitString(UART_DBG, (BYTE *)"Start Search Tiny System\r\n");

	for(BYTE i=slvAdr ; i < MAX_SYSTEMS ; ++i)
	{
	    if( TRUE == I2C_receiveBytes(i, cmd, 2, dat, 1) )
	    {
	    	UART_transmitString(UART_DBG, (BYTE *)"Find tiny System(address = 0x");
	    	UART_transmitByte(UART_DBG, hex2Ascii(i>>4));
	    	UART_transmitByte(UART_DBG, hex2Ascii(i>>0));
	    	UART_transmitString(UART_DBG, (BYTE *)") magic = ");
	    	UART_transmitByte(UART_DBG, hex2Ascii(dat[0]>>4));
	    	UART_transmitByte(UART_DBG, hex2Ascii(dat[0]>>0));
	    	UART_transmitString(UART_DBG, (BYTE *)")\r\n");

	    	tinySystemTable[i] = dat[0];
	    }
	    else
	    {
	    	tinySystemTable[i] = TINY_SYSTEM_INVALID;
	    }
	}

	UART_transmitString(UART_DBG, (BYTE *)"End Search Tiny System\r\n");


	TINY_GPIO adport;
	adport.bit.MUX = TINY_MUX_ADC;
	cmd[1] = TINY_GPIO2_SETTING;
	cmd[2] = adport.byte;

	I2C_transmitBytes(0x03, cmd, 3);



	idx = 0;
	sleepTick = TIMER_systemTick();

	return TRUE;
}


BOOL tinySystem_draw(AksImage *img, AksPoint pos)
{
    AksPixel col = aksPixel(  0, 150, 239);
    AksPixel stpCol = aksPixel(  0,  0,  0);
    AksPixel fwdCol = aksPixel(164,221,255);
    AksPixel bakCol = aksPixel(  0, 73,117);
    AksPixel *l_LegCol;
    AksPixel *r_LegCol;

    BYTE r_legMode = (currentMode >> 0) & 0x0F;
    BYTE l_legMode = (currentMode >> 4) & 0x0F;

    l_LegCol = (l_legMode & DRIVE) ? ((l_legMode & DRIVE_FWD) ? &fwdCol : &bakCol ) : &stpCol;
    r_LegCol = (r_legMode & DRIVE) ? ((r_legMode & DRIVE_FWD) ? &fwdCol : &bakCol ) : &stpCol;


    // body
    aksDrawRect(img, aksPoint(30+pos.x,  5+pos.y), aksPoint(60 + 30+pos.x, 80 +  5+pos.y), col);
    aksDrawRect(img, aksPoint(55+pos.x, 90+pos.y), aksPoint(10 + 55+pos.x, 30 + 90+pos.y), col);
    aksDrawRect(img, aksPoint(30+pos.x,100+pos.y), aksPoint(60 + 30+pos.x, 10 +100+pos.y), col);

    // Legs
    aksDrawRect(img, aksPoint(  5+pos.x, 80+pos.y), aksPoint(20 +  5+pos.x, 50 + 80+pos.y), col);
    akdDrawFilledRect(img,  aksPoint( 6+pos.x,  81+pos.y), aksPoint(19 +  6+pos.x, 49 + 81+pos.y), *r_LegCol);

    aksDrawRect(img, aksPoint( 95+pos.x, 80+pos.y), aksPoint(20 + 95+pos.x, 50 + 80+pos.y), col);
    akdDrawFilledRect(img,  aksPoint(96+pos.x,  81+pos.y), aksPoint(19 + 96+pos.x, 49 + 81+pos.y), *l_LegCol);

    // cards
    aksDrawRect(img, aksPoint(33+pos.x,  10+pos.y), aksPoint(54 + 33+pos.x, 10 + 10+pos.y), col);
    aksDrawRect(img, aksPoint(33+pos.x,  25+pos.y), aksPoint(54 + 33+pos.x, 10 + 25+pos.y), col);
    aksDrawRect(img, aksPoint(33+pos.x,  40+pos.y), aksPoint(54 + 33+pos.x, 10 + 40+pos.y), col);
    aksDrawRect(img, aksPoint(33+pos.x,  55+pos.y), aksPoint(54 + 33+pos.x, 10 + 55+pos.y), col);

    // cam
    aksDrawRect(img, aksPoint(33+pos.x,  75+pos.y), aksPoint(15 + 33+pos.x,  5 + 75+pos.y), col);
    akdDrawFilledRect(img,  aksPoint(34+pos.x,  76+pos.y), aksPoint(14 + 34+pos.x,  4 + 76+pos.y), aksPixel(0,0,255));

    return TRUE;
}
